

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <stdbool.h>

#include "rp2040.h"
#include "drvs/resets.h"
#include "drvs/pio.h"
#include "drvs/pio_instructions.h"

#include "umsg.h"


#if 0

static const uint16_t mdio_program_instructions[] = {
            //     .wrap_target
    0xe080, //  0: set    pindirs, 0                 
    0x80a0, //  1: pull   block                      
    0xe001, //  2: set    pins, 1                    
    0xe081, //  3: set    pindirs, 1                 
    0x6042, //  4: out    y, 2                       
    0xe036, //  5: set    x, 22                      
    0xb442, //  6: nop                    side 0 [4] 
    0xbc42, //  7: nop                    side 1 [4] 
    0xb442, //  8: nop                    side 0 [4] 
    0x1c46, //  9: jmp    x--, 6          side 1 [4] 
    0xf400, // 10: set    pins, 0         side 0 [4] 
    0xbc42, // 11: nop                    side 1 [4] 
    0xb442, // 12: nop                    side 0 [4] 
    0xfa2b, // 13: set    x, 11           side 1 [2] 
    0xa042, // 14: nop                               
    0xa042, // 15: nop                               
    0x7401, // 16: out    pins, 1         side 0 [4] 
    0x1a4e, // 17: jmp    x--, 14         side 1 [2] 
    0x0077, // 18: jmp    !y, 23                     
    0xe031, // 19: set    x, 17                      
    0x7401, // 20: out    pins, 1         side 0 [4] 
    0x1c54, // 21: jmp    x--, 20         side 1 [4] 
    0x001d, // 22: jmp    29                         
    0xa042, // 23: nop                               
    0xf380, // 24: set    pindirs, 0      side 0 [3] 
    0xe001, // 25: set    pins, 1                    
    0xfc30, // 26: set    x, 16           side 1 [4] 
    0x5401, // 27: in     pins, 1         side 0 [4] 
    0x1c5b, // 28: jmp    x--, 27         side 1 [4] 
    0x8020, // 29: push   block                      
    0x0000, // 30: jmp    0                          
            //     .wrap
};


static const struct pio_program mdio_program = {
    .instructions = mdio_program_instructions,
    .length = 31,
    .origin = -1,
};




static uint32_t mdio_encode_address( uint8_t phyadr, uint8_t devtyp, uint16_t data )
{
    uint32_t temp;

    temp = 3;                       // write or read = 3
    temp = (temp << 2) | 0b00;      // write address
    temp = (temp << 5) | (phyadr & 0x1F);   // phy address
    temp = (temp << 5) | (devtyp & 0x1F);   // phy address
    
    temp = (temp << 2) | 0b10;      // turn
    temp = (temp << 16) | data;     // data

    return temp;
}


static uint32_t mdio_encode_write( uint8_t phyadr, uint8_t devtyp, uint16_t data )
{
	uint32_t temp;

	temp = 3;						// write or read = 3
	temp = (temp << 2) | 0b01;		// write data
	temp = (temp << 5) | (phyadr & 0x1F);	// phy address
	temp = (temp << 5) | (devtyp & 0x1F);	// phy address
	
	temp = (temp << 2) | 0b10;		// turn
	temp = (temp << 16) | data;		// data

	return temp;
}

static uint32_t mdio_encode_read( bool post, uint8_t phyadr, uint8_t devtyp )
{
	uint32_t temp;

	temp = 0;						// write or read = 0

    if ( post ) {
        temp = (temp << 2) | 0b10;      // post read
    } else {
        temp = (temp << 2) | 0b11;      // read
    }
	
	temp = (temp << 5) | (phyadr & 0x1F);	// phy address
	temp = (temp << 5) | (devtyp & 0x1F);	// phy address
	temp = temp << 18;				// zero 18 bits.

	return temp;
}



int mdio_address( uint8_t phyadr, uint8_t devtyp, uint16_t addr )
{
    PIO pio = pio0;
    uint smx = 0;
    uint rcnt;
    uint32_t temp;

    /**/
    temp = mdio_encode_address( phyadr, devtyp, addr );
    pio_sm_put_blocking( pio, smx, temp );

    /* check rx level */
    while ( true ) {
        rcnt = pio_sm_get_rx_fifo_level( pio, smx );
        if ( rcnt > 0 ) {
            break;
        }
    }
    
    /* eat clean */
    pio_sm_get( pio, smx );
    return 0;
}



int mdio_write( uint8_t phyadr, uint8_t devtyp, uint16_t data )
{
	PIO pio = pio0;
    uint smx = 0;
    uint rcnt;
	uint32_t temp;

    /**/
    temp = mdio_encode_write( phyadr, devtyp, data );
    pio_sm_put_blocking( pio, smx, temp );

    /* check rx level */
    while ( true ) {
    	rcnt = pio_sm_get_rx_fifo_level( pio, smx );
    	if ( rcnt > 0 ) {
    		break;
    	}
	}
    
    /* eat clean */
    pio_sm_get( pio, smx );
    return 0;
}


int mdio_read( uint8_t phyadr, uint8_t devtyp, uint16_t * pdat )
{
	PIO pio = pio0;
    uint smx = 0;
    uint32_t temp;
    uint rcnt;

    /**/
    temp = mdio_encode_read( false, phyadr, devtyp );
    pio_sm_put_blocking( pio, smx, temp );

    /* check rx level */
    while ( true ) {
    	rcnt = pio_sm_get_rx_fifo_level( pio, smx );
    	if ( rcnt > 0 ) {
    		break;
    	}
	}
    
    /* get result */
    temp = pio_sm_get( pio, smx );
    *pdat = (uint16_t)temp;
    return 0;
}


int mdio_postrd( uint8_t phyadr, uint8_t devtyp, uint16_t * pdat )
{
    PIO pio = pio0;
    uint smx = 0;
    uint32_t temp;
    uint rcnt;

    /**/
    temp = mdio_encode_read( true, phyadr, devtyp );
    pio_sm_put_blocking( pio, smx, temp );

    /* check rx level */
    while ( true ) {
        rcnt = pio_sm_get_rx_fifo_level( pio, smx );
        if ( rcnt > 0 ) {
            break;
        }
    }
    
    /* get result */
    temp = pio_sm_get( pio, smx );
    *pdat = (uint16_t)temp;
    return 0;
}

#endif


static const uint16_t mdio_program_instructions[] = {
            //     .wrap_target
    0x80a0, //  0: pull   block                      
    0x6026, //  1: out    x, 6                       
    0x6041, //  2: out    y, 1                       
    0xa342, //  3: nop                           [3] 
    0xf401, //  4: set    pins, 1         side 0 [4] 
    0x1843, //  5: jmp    x--, 3          side 1     
    0xe02d, //  6: set    x, 13                      
    0xa142, //  7: nop                           [1] 
    0x7401, //  8: out    pins, 1         side 0 [4] 
    0x1a47, //  9: jmp    x--, 7          side 1 [2] 
    0x0072, // 10: jmp    !y, 18                     
    0xe230, // 11: set    x, 16                  [2] 
    0xf480, // 12: set    pindirs, 0      side 0 [4] 
    0xfc01, // 13: set    pins, 1         side 1 [4] 
    0x5301, // 14: in     pins, 1         side 0 [3] 
    0x004d, // 15: jmp    x--, 13                    
    0xfa81, // 16: set    pindirs, 1      side 1 [2] 
    0x0019, // 17: jmp    25                         
    0x80a0, // 18: pull   block                      
    0x7401, // 19: out    pins, 1         side 0 [4] 
    0x7825, // 20: out    x, 5            side 1     
    0xa342, // 21: nop                           [3] 
    0x7401, // 22: out    pins, 1         side 0 [4] 
    0x1855, // 23: jmp    x--, 21         side 1     
    0xa042, // 24: nop                               
    0x6025, // 25: out    x, 5                       
    0x003d, // 26: jmp    !x, 29                     
    0xf401, // 27: set    pins, 1         side 0 [4] 
    0x1c5b, // 28: jmp    x--, 27         side 1 [4] 
    0x8020, // 29: push   block                      
    0x0000, // 30: jmp    0                          
            //     .wrap
};


static const struct pio_program mdio_program = {
    .instructions = mdio_program_instructions,
    .length = 31,
    .origin = -1,
};


/*
addr = true  : address trans
addr = false :   write trans
*/
static int mdio_encode_write( bool addr, uint8_t phyadr, uint8_t devtyp, uint16_t data, uint32_t * pary )
{
    uint32_t temp;

    temp = 50u << 1;                    // idle + pre = 40, 0 = write
    if ( addr )  {
        temp = (temp << 4) | 0;         // ST=00, OP=00
    } else {
        temp = (temp << 4) | 0x01;      // ST=00, OP=01
    }
    
    temp = (temp << 5) | (phyadr & 0x1F);
    temp = (temp << 5) | (devtyp & 0x1F);
    pary[0] = temp << 11;


    temp = (1u << 5) | 16u;
    temp = (temp << 17) | data;
    pary[1] = temp << 9;
    return 2;
}


/*
post = true  : post read trans
post = false : read trans
*/
static int mdio_encode_read( bool post, uint8_t phyadr, uint8_t devtyp, uint32_t * pary )
{
    uint32_t temp;

    temp = (50u << 1) | 0x1;            // idle + pre = 40, 1 = read
    if ( post )  {
        temp = (temp << 4) | 0x02;      // ST=00, OP=10
    } else {
        temp = (temp << 4) | 0x03;      // ST=00, OP=11
    }
    
    temp = (temp << 5) | (phyadr & 0x1F);
    temp = (temp << 5) | (devtyp & 0x1F);
    pary[0] = temp << 11;

    /**/
    return 1;
}


int mdio_address( uint8_t phyadr, uint8_t devtyp, uint16_t addr )
{
    PIO pio = pio0;
    uint smx = 0;
    int iret;
    uint32_t tary[4];

    iret = mdio_encode_write( true, phyadr, devtyp, addr, tary );
    for ( int i=0; i<iret; i++ ) {
        pio_sm_put_blocking( pio, smx, tary[i] );
    }

    /* wait, get result */
    pio_sm_get_blocking( pio, smx );
    return 0;
}



int mdio_write( uint8_t phyadr, uint8_t devtyp, uint16_t data )
{
    PIO pio = pio0;
    uint smx = 0;
    int iret;
    uint32_t tary[4];

    iret = mdio_encode_write( false, phyadr, devtyp, data, tary );
    for ( int i=0; i<iret; i++ ) {
        pio_sm_put_blocking( pio, smx, tary[i] );
    }

    /* wait, get result */
    pio_sm_get_blocking( pio, smx );    
    return 0;
}


int mdio_read( uint8_t phyadr, uint8_t devtyp, uint32_t * pdat )
{
    PIO pio = pio0;
    uint smx = 0;
    int iret;
    uint32_t tary[4];

    iret = mdio_encode_read( false, phyadr, devtyp, tary );
    for ( int i=0; i<iret; i++ ) {
        pio_sm_put_blocking( pio, smx, tary[i] );
    }

    /* wait, get result */
    *pdat = pio_sm_get_blocking( pio, smx );
    return 0;
}


int mdio_postrd( uint8_t phyadr, uint8_t devtyp, uint32_t * pdat )
{
    PIO pio = pio0;
    uint smx = 0;
    int iret;
    uint32_t tary[4];

    iret = mdio_encode_read( true, phyadr, devtyp, tary );
    for ( int i=0; i<iret; i++ ) {
        pio_sm_put_blocking( pio, smx, tary[i] );
    }

    /* wait, get result */
    *pdat = pio_sm_get_blocking( pio, smx );
    return 0;
}


int mdio_intf_init( uint mck, uint mio, uint32_t freq )
{
    PIO pio = pio0;
    uint sm = 0;
    uint clk_pin = mck;
    uint dio_pin = mio;
    pio_sm_config cfg;

    /* unreset */
    unreset_block_wait( RESETS_RESET_PIO0_BITS );

    /* gpio : mck */
    gpio_disable_pulls( clk_pin );
    pio_gpio_init( pio, clk_pin );

    /* gpio : mdio */
    gpio_disable_pulls( dio_pin );
    pio_gpio_init( pio, dio_pin );

    /* default : pindir and pins */
    pio_sm_set_consecutive_pindirs(pio, sm, clk_pin, 1, true );
    pio_sm_set_consecutive_pindirs(pio, sm, dio_pin, 1, true );
    pio_sm_set_pins( pio, sm, (1 << clk_pin) | (1 << dio_pin) );

    /* state machine, config */
    cfg = pio_get_default_sm_config();
    sm_config_set_out_pins( &cfg, dio_pin, 1 );
    sm_config_set_set_pins( &cfg, dio_pin, 1 );
    sm_config_set_in_pins( &cfg, dio_pin );
    sm_config_set_sideset_pins( &cfg, clk_pin );
    sm_config_set_sideset( &cfg, 2, true, false );
    sm_config_set_clkdiv_int_frac( &cfg, 20, 0 );
    sm_config_set_out_shift( &cfg, false, false, 32 );
    sm_config_set_in_shift( &cfg, false, false, 32 );
    sm_config_set_wrap( &cfg, 0, (mdio_program.length - 1) );

    /* INST_MEM */
    pio_add_program_at_offset( pio, &mdio_program, 0 );

    /**/
    pio_sm_init( pio, sm, 0, &cfg );
    pio_sm_set_enabled( pio, sm, true );

    /* IRQ enable */
    //pio_set_irq0_source_enabled( pio, pis_interrupt0, true );
    //NVIC_EnableIRQ( PIO0_IRQ_0_IRQn );
    return 0;
}
